# this module help calc data

import queue
import sys
import threading

import warnings

import location_init
from location_init import *

import time

# out_file = open(location_init.out_dir+"/detail.txt","w")
# # print('LocationCalculater',location_init.out_dir)
# sys.stdout = out_file

class LocationCalculater:
    def __init__(self,server):
        print("__init__ LocationCalculater start")
        self.info_queue = queue.Queue(-1)

        self.sensor_lock = threading.Lock()
        self.sensor_info = queue.Queue(5)

        self.query_info = queue.Queue(-1)

        self.location_log = queue.Queue(2000)

        self.server = server
        self.bind_map = None

        self.location = location_list.LocationList(200,name='Guard',need_sim_pair=True)
        self.location_sim = self.location._sim_pair

        print("self.location size =",self.location.size)
        print("self.location_sim size =",self.location_sim.size)

        self.update_lock = threading.Lock()
        self.need_recover = False

        # self.updata_thread stop with server's recv_thread stop
        self.update_thread=threading.Thread(target=self._location_update,args=())
        self.update_thread.daemon = True 
        self.update_thread.start()

        self.start_timer = False
        self.time_list = {"send":-1,"calc":-1,"sim":-1}

        print("__init__ LocationCalculater end")


    def _location_update(self):
        while True:
            term_id,data_type,elems = self.info_queue.get()
            self.system_update(term_id,data_type,elems)
            self.bind_map_update()

            if (self.start_timer):
                if (self.time_list["calc"]>0):
                    self.time_list["sim"] = time.time()
                    self._print_delay()
                    self.start_timer = False
                
            if (self.server._shutdown):break
            # print(1)
            # time.sleep(2)
        # print('end')
    
    def _print_delay(self):
        a,b,c = self.time_list['send'],self.time_list['calc'],self.time_list['sim']
        if (c-a > 0.2): 
            s = "delay: {:.6f} {}".format(c-a,self.time_list)
            # warnings.warn(s,UserWarning)
        print("delay\n"*10)
        print(self.time_list)
        print(b-a,c-b,c-a)
    
    def bind_map_update(self):
        if self.bind_map:
            sim_irs_pos = self.location_sim.current_postion
            self.bind_map.update(sim_irs_pos.dict_style,self.start_timer,self.time_list["send"])

    def system_update(self,term_id,data_type,elems):
        if data_type == "SYSTEM_TYPE":
            self.handle_system_data(elems)
            return 

        elif data_type == "SENSOR_TYPE":
            # 1) get 4 sensor
            ret = [term_id]
            for i in range(0,4):
                ret.append(int(elems[i]))

            # 1.5) get send timestamp
            send_t = float(elems[4]) 

            # 2) if has tag
            need_location = True if len(elems)>5 else False

            ret.append(need_location)
            if (need_location):
                print("[SNAPSHOT] need location\n"*10)         

            if (not self.start_timer and self.sensor_info.empty()):  
                self.start_timer = True
                for k in self.time_list:
                    self.time_list[k] = -1
                self.time_list["send"] = send_t

            # 3) append sensor info, if full advance once
            self.sensor_lock.acquire()

            self.sensor_info.put(ret)
            if (self.sensor_info.full()):
                sensor_data,with_tag = self.get_sensor_group()
                msg = []

                self.location.advance_once(sensor_data,msg)
                if (self.start_timer):
                    self.time_list["calc"] = time.time()
            
                if (self.location.is_unmovable):
                    while (not self.query_info.empty()):
                        qid = self.query_info.get()
                        self.make_unmovable_reply(qid)

                for tid in with_tag:
                    self.make_snapshot_reply(tid)

            self.sensor_lock.release()

            #4) if has tag, must "SYSTEM_TYPE,ADJUST,on"
            if need_location:
                # 
                l = ["ADJUST","on"]
                self.handle_system_data(l)

            return
            
        elif data_type == "ACTION_TYPE":
            self.handle_action_data(elems)
            return 

        elif data_type == "ANGLE_TYPE":
            self.handle_angle_data(float(elems[0]),float(elems[1]))
            return 

        elif data_type == "QUERY":
            print("[QUERY] need location\n"*10)
            # TODO set an threading reply by the first unmovable
            query_type = elems[0]
            if (query_type=='position'):
                self.query_info.put(term_id)
            else:
                raise Exception("unknown query {} {} {}".format(term_id,data_type,elems))

        elif data_type == "EOF":
            print("shutdown")
            self.server._shutdown = True
            return 

        else:
            # unknown legal type
            raise Exception("Unknown date type {} {} {}".format(term_id,data_type,elems))


    def once_update(self,group_cnt=5):
        if (self.need_recover): 
            raise Exception("need recover!".format())

        st_time = time.time()

        # 1) get sensor_info
        sensor_data,reply_terms = self.get_sensor_group(group_cnt)
    
        msg = []
        self.location.advance_once(sensor_data,msg)
        if (self.location.need_recover):
            self.need_recover = True
        
        location = self.get_newest_location(self)

        for term_id in reply_terms:
            self.server.append_reply(term_id,location)

        ed_time = time.time()
        print("system one step time = {:.6f}".format(ed_time-st_time))

    def get_sensor_group(self):
        sensor_data = [0,0,0,0]

        with_tag = []
        group_cnt=0

        while(not self.sensor_info.empty()):
            ss = self.sensor_info.get()
            group_cnt+=1

            term_id,distance,tag = ss[0],ss[1:5],ss[5]
            
            for i in range(4):
                sensor_data[i] += distance[i]

            if tag: with_tag.append(term_id)
        
        for i in range(4):
            sensor_data[i]/=group_cnt

        return sensor_data,with_tag

    def handle_system_data(self,info):
        self.location.set_system_state(info)
        pass

    def handle_action_data(self,info):
        self.location.set_action(info)
        pass

    def handle_angle_data(self,angle:float,raw_angle:float):
        self.location.set_sdk_angle(angle)
        self.location.set_raw_sdk_angle(raw_angle)

    def make_unmovable_reply(self,qid):
        assert(self.location.is_unmovable)
        pos = self.location.current_postion
        pos_msg = "{},{:.3f},{:.3f},{:.3f}".format(
            self.location.location_analysis,
            pos.x,pos.y,pos.deg
        )
        reply_msg = "{} {} {}".format(
            qid,
            "position",
            pos_msg
        )
        self.server._send_msg_queue.put(reply_msg)


    def make_snapshot_reply(self,qid):
        # assert(self.location.is_unmovable)
        pos = self.location.current_postion
        pos_msg = "{},{:.3f},{:.3f},{:.3f}".format(
            self.location.location_analysis,
            pos.x,pos.y,pos.deg
        )
        reply_msg = "{} {} {}".format(
            qid,
            "snapshot",
            pos_msg
        )
        self.server._send_msg_queue.put(reply_msg)


